/*NNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNN

	Copyright (c) 2009 Jounayd Id Salah.
	All rights reserved.

	This file is part of Blacklib.

	Blacklib is free software: you can redistribute it and/or modify it under the 
	terms of the GNU Lesser General Public License as published by the Free Software 
	Foundation, either version 3 of the License, or (at your option) any later version.
	Blacklib is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; 
	without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 
	PURPOSE.  See the GNU Lesser General Public License for more details.
	You should have received a copy of the GNU Lesser General Public License along with 
	Blacklib.  If not, see <http://www.gnu.org/licenses/>.
*/
//NNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNN

#include "maths/matrix.h"

#include "maths/trigo.h"

//#include <fvec.h>

namespace black
{
	//***************************************************************
	// mat3x3_128
	//***************************************************************

	//__________________________________________________________________________
	bALIGN16 const mat3x3_128 mat3x3_128::ZERO (		0.f, 0.f, 0.f,
		0.f, 0.f, 0.f,
		0.f, 0.f, 0.f );

	bALIGN16 const mat3x3_128 mat3x3_128::IDENTITY (	1.f, 0.f, 0.f,
		0.f, 1.f, 0.f,
		0.f, 0.f, 1.f );

	//__________________________________________________________________________
	/*mat3x3_128 mat3x3_128::operator* ( const mat3x3_128& m ) const
	{
#ifdef bSSE
		mat3x3_128 res;

		const __m128 m0 = _128.i;
		const __m128 m1 = _128.j;
		const __m128 m2 = _128.z;

		__m128 mm0 = _mm_set_ps1( m._.ix );
		__m128 mm1 = _mm_set_ps1( m._.iy );
		__m128 mm2 = _mm_set_ps1( m._.iz );

		res._128.i		= _mm_mul_ps( m0, mm0 );
		__m128 mmTmp1	= _mm_mul_ps( m1, mm1 );
		res._128.i		= _mm_add_ps( res._128.i, mmTmp1 );
		__m128 mmTmp2	= _mm_mul_ps( m2, mm2 );
		res._128.i		= _mm_add_ps( res._128.i, mmTmp2 );

		mm0 = _mm_set_ps1( m._.jx );
		mm1 = _mm_set_ps1( m._.jy );
		mm2 = _mm_set_ps1( m._.jz );

		res._128.j		= _mm_mul_ps( m0, mm0 );
		mmTmp1			= _mm_mul_ps( m1, mm1 );
		res._128.j		= _mm_add_ps( res._128.j, mmTmp1 );
		mmTmp2			= _mm_mul_ps( m2, mm2 );
		res._128.j		= _mm_add_ps( res._128.j, mmTmp2 );

		mm0 = _mm_set_ps1( m._.kx );
		mm1 = _mm_set_ps1( m._.ky );
		mm2 = _mm_set_ps1( m._.kz );

		res._128.z	= _mm_mul_ps( m0, mm0 );
		mmTmp1		= _mm_mul_ps( m1, mm1 );
		res._128.z	= _mm_add_ps( res._128.z, mmTmp1 );
		mmTmp2		= _mm_mul_ps( m2, mm2 );
		res._128.z	= _mm_add_ps( res._128.z, mmTmp2 );

		return res;
#else
		return mat3x3_128(
			_.ix*m._.ix + _.jx*m._.iy + _.kx*m._.iz, _.ix*m._.jx + _.jx*m._.jy + _.kx*m._.jz, _.ix*m._.kx + _.jx*m._.ky + _.kx*m._.kz,
			_.iy*m._.ix + _.jy*m._.iy + _.ky*m._.iz, _.iy*m._.jx + _.jy*m._.jy + _.ky*m._.jz, _.iy*m._.kx + _.jy*m._.ky + _.ky*m._.kz,
			_.iz*m._.ix + _.jz*m._.iy + _.kz*m._.iz, _.iz*m._.jx + _.jz*m._.jy + _.kz*m._.jz, _.iz*m._.kx + _.jz*m._.ky + _.kz*m._.kz
			);
#endif
	}*/

	//__________________________________________________________________________
	void mat3x3_128::setMul ( const mat3x3_128& m1, const mat3x3_128& m2 )
	{
		_.ix = m1._.ix*m2._.ix + m1._.jx*m2._.iy + m1._.kx*m2._.iz;
		_.jx = m1._.ix*m2._.jx + m1._.jx*m2._.jy + m1._.kx*m2._.jz;
		_.kx = m1._.ix*m2._.kx + m1._.jx*m2._.ky + m1._.kx*m2._.kz;

		_.iy = m1._.iy*m2._.ix + m1._.jy*m2._.iy + m1._.ky*m2._.iz;
		_.jy = m1._.iy*m2._.jx + m1._.jy*m2._.jy + m1._.ky*m2._.jz;
		_.ky = m1._.iy*m2._.kx + m1._.jy*m2._.ky + m1._.ky*m2._.kz;

		_.iz = m1._.iz*m2._.ix + m1._.jz*m2._.iy + m1._.kz*m2._.iz;
		_.jz = m1._.iz*m2._.jx + m1._.jz*m2._.jy + m1._.kz*m2._.jz;
		_.kz = m1._.iz*m2._.kx + m1._.jz*m2._.ky + m1._.kz*m2._.kz;
	}

	//__________________________________________________________________________
	void mat3x3_128::mul ( vec3_128& vRes, const vec3_128& vIn ) const
	{
		vRes.x = _.ix*vIn.x + _.jx*vIn.y + _.kx*vIn.z;
		vRes.y = _.iy*vIn.x + _.jy*vIn.y + _.ky*vIn.z;
		vRes.z = _.iz*vIn.x + _.jz*vIn.y + _.kz*vIn.z;
	}

	//__________________________________________________________________________
	// 24% faster than setMul
	void mat3x3_128::setMulsse ( const mat3x3_128& m1, const mat3x3_128& m2 )
	{
		assertAligned16Ptr( &m1 );
		assertAligned16Ptr( &m2 );
		assertAligned16Ptr( this );
		const __m128 mm1_0 = m1._128.i;
		const __m128 mm1_1 = m1._128.j;
		const __m128 mm1_2 = m1._128.k;

		__m128 mm2_0;
		__m128 mm2_1;
		__m128 mm2_2;

		__m128 mmTmp1;
		__m128 mmTmp2;

		mm2_0	= _mm_set_ps1( m2._.ix );
		mm2_1	= _mm_set_ps1( m2._.iy );
		mm2_2	= _mm_set_ps1( m2._.iz );

		_128.i	= _mm_mul_ps( mm1_0, mm2_0 );
		mmTmp1	= _mm_mul_ps( mm1_1, mm2_1 );
		_128.i	= _mm_add_ps( _128.i, mmTmp1 );
		mmTmp2	= _mm_mul_ps( mm1_2, mm2_2 );
		_128.i	= _mm_add_ps( _128.i, mmTmp2 );

		mm2_0	= _mm_set_ps1( m2._.jx );
		mm2_1	= _mm_set_ps1( m2._.jy );
		mm2_2	= _mm_set_ps1( m2._.jz );

		_128.j	= _mm_mul_ps( mm1_0, mm2_0 );
		mmTmp1	= _mm_mul_ps( mm1_1, mm2_1 );
		_128.j	= _mm_add_ps( _128.j, mmTmp1 );
		mmTmp2	= _mm_mul_ps( mm1_2, mm2_2 );
		_128.j	= _mm_add_ps( _128.j, mmTmp2 );

		mm2_0	= _mm_set_ps1( m2._.kx );
		mm2_1	= _mm_set_ps1( m2._.ky );
		mm2_2	= _mm_set_ps1( m2._.kz );

		_128.k	= _mm_mul_ps( mm1_0, mm2_0 );
		mmTmp1	= _mm_mul_ps( mm1_1, mm2_1 );
		_128.k	= _mm_add_ps( _128.k, mmTmp1 );
		mmTmp2	= _mm_mul_ps( mm1_2, mm2_2 );
		_128.k	= _mm_add_ps( _128.k, mmTmp2 );
	}

	//__________________________________________________________________________
	void mat3x3_128::mulsse( vec3_128& vRes, const vec3_128& vIn ) const
	{
		assertAligned16Ptr( &vRes );
		assertAligned16Ptr( &vIn );
		/*const __m128 mm1_0 = _128.i;
		const __m128 mm1_1 = _128.j;
		const __m128 mm1_2 = _128.k;*/

		__m128 mm2_0;
		__m128 mm2_1;
		__m128 mm2_2;

		__m128 mmTmp1;
		__m128 mmTmp2;

		mm2_0	= _mm_set_ps1( vIn.x );
		mm2_1	= _mm_set_ps1( vIn.y );
		mm2_2	= _mm_set_ps1( vIn.z );

		vRes	= _mm_mul_ps( _128.i, mm2_0 );
		mmTmp1	= _mm_mul_ps( _128.j, mm2_1 );
		vRes	= _mm_add_ps( vRes, mmTmp1 );
		mmTmp2	= _mm_mul_ps( _128.k, mm2_2 );
		vRes	= _mm_add_ps( vRes, mmTmp2 );
	}

	//__________________________________________________________________________
	void mat3x3_128::transpose()
	{
		register float fTmp	= _.jx; _.jx = _.iy; _.iy = fTmp;
		fTmp = _.kx; _.kx = _.iz; _.iz = fTmp;
		fTmp = _.ky; _.ky = _.jz; _.jz = fTmp;
	}

	//__________________________________________________________________________
	void mat3x3_128::transposesse()
	{
		assertAligned16Ptr( this );
		__m128 tmp3, tmp2, tmp1, tmp0;
		tmp0   = _mm_shuffle_ps( _128.i, _128.j, 0x44 );
		tmp2   = _mm_shuffle_ps( _128.i, _128.j, 0xEE );
		tmp1   = _mm_shuffle_ps( _128.k, _128.k, 0x44 );
		tmp3   = _mm_shuffle_ps( _128.k, _128.k, 0xEE );
		_128.i = _mm_shuffle_ps( tmp0, tmp1, 0x88 );
		_128.j = _mm_shuffle_ps( tmp0, tmp1, 0xDD );
		_128.k = _mm_shuffle_ps( tmp2, tmp3, 0x88 );
		//_128.m3 = _mm_shuffle_ps( tmp2, tmp3, 0xDD );
	}

	//__________________________________________________________________________
	void mat3x3_128::setRotX( const float fAngle )
	{
		float fSin, fCos;
		sinCos( fSin, fCos, fAngle );

		_128.i = IDENTITY.i();
		_.jx = 0.f; _.jy =  fCos; _.jz = fSin;
		_.kx = 0.f; _.ky = -fSin; _.kz = fCos;
	}

	//__________________________________________________________________________
	void mat3x3_128::setRotY( const float fAngle )
	{
		float fSin, fCos;
		sinCos( fSin, fCos, fAngle );

		_.ix = fCos; _.iy = 0.f; _.iz = -fSin;
		_128.j = IDENTITY.j();
		_.kx = fSin; _.ky = 0.f; _.kz =  fCos;
	}

	//__________________________________________________________________________
	void mat3x3_128::setRotZ( const float fAngle )
	{
		float fSin, fCos;
		sinCos( fSin, fCos, fAngle );

		_.ix =  fCos; _.iy = fSin; _.iz = 0.f;
		_.jx = -fSin; _.jy = fCos; _.jz = 0.f;
		_128.k = IDENTITY.k();
	}

	//__________________________________________________________________________
	void mat3x3_128::invert()
	{
		const float fDet = determinant();
		assert( fDet != 0.f );
		const float fInvDet = 1.0f / fDet;

		mat3x3_128 mTemp;
		mTemp._.ix =  (_.jy * _.kz - _.ky * _.jz) * fInvDet;
		mTemp._.iy = -(_.jx * _.kz - _.kx * _.jz) * fInvDet;
		mTemp._.iz =  (_.jx * _.ky - _.kx * _.jy) * fInvDet;

		mTemp._.jx = -(_.iy * _.kz - _.ky * _.iz) * fInvDet;
		mTemp._.jy =  (_.ix * _.kz - _.kx * _.iz) * fInvDet;
		mTemp._.jz = -(_.ix * _.ky - _.kx * _.iy) * fInvDet;

		mTemp._.kx =  (_.iy * _.jz - _.jy * _.iz) * fInvDet;
		mTemp._.ky = -(_.ix * _.jz - _.jx * _.iz) * fInvDet;
		mTemp._.kz =  (_.ix * _.jy - _.jx * _.iy) * fInvDet;

		*this = mTemp;
	}

	//__________________________________________________________________________
	// 47% than invert
	void mat3x3_128::invertsse()
	{
		static const __m128 mNegy = _mm_set_ps( 1.f, -1.f, 1.f, 0.f );
		static const __m128 mNegxz = _mm_set_ps( -1.f, 1.f, -1.f, 0.f );

		assertAligned16Ptr( this );
		const float fDet = determinant();
		assert( fDet != 0.f );

		__m128 mmInvDet = _mm_set1_ps( fDet );
		mmInvDet = _mm_rcp_ps( mmInvDet );

		mat3x3_128 mTemp;

		__m128 mmIy = _mm_shuffle_ps( _128.i, _128.i, _MM_SHUFFLE(0, 0, 0, 1) );
		__m128 mmIz = _mm_shuffle_ps( _128.i, _128.i, _MM_SHUFFLE(0, 1, 2, 2) );
		__m128 mmJy = _mm_shuffle_ps( _128.j, _128.j, _MM_SHUFFLE(0, 0, 0, 1) );
		__m128 mmJz = _mm_shuffle_ps( _128.j, _128.j, _MM_SHUFFLE(0, 1, 2, 2) );
		__m128 mmKy = _mm_shuffle_ps( _128.k, _128.k, _MM_SHUFFLE(0, 0, 0, 1) );
		__m128 mmKz = _mm_shuffle_ps( _128.k, _128.k, _MM_SHUFFLE(0, 1, 2, 2) );		

		__m128 mmTmp;
		mTemp._128.i	= _mm_mul_ps( mmJy, mmKz );
		mmTmp			= _mm_mul_ps( mmKy, mmJz );
		mTemp._128.i	= _mm_sub_ps( mTemp._128.i, mmTmp );

		mTemp._128.j	= _mm_mul_ps( mmIy, mmKz );
		mmTmp			= _mm_mul_ps( mmKy, mmIz );
		mTemp._128.j	= _mm_sub_ps( mTemp._128.j, mmTmp );

		mTemp._128.k	= _mm_mul_ps( mmIy, mmJz );
		mmTmp			= _mm_mul_ps( mmJy, mmIz );
		mTemp._128.k	= _mm_sub_ps( mTemp._128.k, mmTmp );

		mTemp._128.i	= _mm_mul_ps( mTemp._128.i, mmInvDet );
		mTemp._128.j	= _mm_mul_ps( mTemp._128.j, mmInvDet );
		mTemp._128.k	= _mm_mul_ps( mTemp._128.k, mmInvDet );

		/*__m128 mNegI	= _mm_sub_ps( vec4_128::ZERO, mTemp._128.i );
		__m128 mNegJ	= _mm_sub_ps( vec4_128::ZERO, mTemp._128.i );
		__m128 mNegK	= _mm_sub_ps( vec4_128::ZERO, mTemp._128.i );*/

		/*const __m128 mmInvDetNegxz	= _mm_mul_ps( mmInvDet, mNegxz );
		const __m128 mmInvDetNegy	= _mm_mul_ps( mmInvDet, mNegy );*/
		
		mTemp._128.i = _mm_mul_ps( mTemp._128.i, mNegy );
		mTemp._128.j = _mm_mul_ps( mTemp._128.j, mNegxz );
		mTemp._128.k = _mm_mul_ps( mTemp._128.k, mNegy );

		*this = mTemp;
	}

	//__________________________________________________________________________
	bool mat3x3_128::isValid() const
	{
		return black::isValid( _.ix ) && black::isValid( _.jx ) && black::isValid( _.kx )
			&& black::isValid( _.iy ) && black::isValid( _.jy ) && black::isValid( _.ky )
			&& black::isValid( _.iz ) && black::isValid( _.jz ) && black::isValid( _.kz );
	}

	//***************************************************************
	// mat4x4_128
	//***************************************************************

	//__________________________________________________________________________
	bool mat4x4_128::isValid() const
	{
		return black::isValid( _.ix ) && black::isValid( _.jx ) && black::isValid( _.kx ) && black::isValid( _.ox )
			&& black::isValid( _.iy ) && black::isValid( _.jy ) && black::isValid( _.ky ) && black::isValid( _.oy )
			&& black::isValid( _.iz ) && black::isValid( _.jz ) && black::isValid( _.kz ) && black::isValid( _.oz )
			&& black::isValid( _.iw ) && black::isValid( _.jw ) && black::isValid( _.kw ) && black::isValid( _.ow );
	}

	//***************************************************************
	// mat3x4_128
	//***************************************************************
}
